#include "MyMath.hpp"
#include <iostream>

//三维矩阵
Mat3::Mat3(){
    m[0][0] = m[1][1] = m[2][2] = 1.0f;
    m[0][1] = m[0][2] = m[1][0] = m[1][2] = m[2][0] = m[2][1] = 0.0f;
}

Mat3::Mat3(float mat[3][3]){
    memcpy(m, mat, 9 * sizeof(float));
}

Mat3::Mat3(float t00, float t01, float t02,
         float t10, float t11, float t12,
         float t20, float t21, float t22){
    m[0][0] = t00, m[0][1] = t01, m[0][2] = t02;
	m[1][0] = t10, m[1][1] = t11, m[1][2] = t12;
	m[2][0] = t20, m[2][1] = t21, m[2][2] = t22;     
}

template <typename T>
Mat3::Mat3(Vector3<T> v0, Vector3<T> v1, Vector3<T> v2){
    m[0][0] = v0.x, m[0][1] = v1.x, m[0][2] = v2.x;
	m[1][0] = v0.y, m[1][1] = v1.y, m[1][2] = v2.y;
	m[2][0] = v0.z, m[2][1] = v1.z, m[2][2] = v2.z;
}

Mat3 Mat3::operator+(const Mat3 &m) const{
    Mat3 ans;
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            ans.m[i][j] = this->m[i][j] + m.m[i][j];
        }
    }
    return ans;
}

Mat3 Mat3::operator*(const Mat3 &m) const{
    Mat3 out;
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            out.m[i][j] = this->m[i][0] * m.m[0][j] +
                this->m[i][1] * m.m[1][j] +
                this->m[i][2] * m.m[2][j];
        }
    }
    return out;
}

Mat3 Mat3::operator*(float n) const{
    Mat3 mat;
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            mat.m[i][j] = m[i][j] * n;
    return mat;
}

Mat3 &Mat3::Transpose(){
    for (int i = 0; i < 3; ++i) 
        for (int j = i + 1; j < 3; ++j) 
            std::swap(m[i][j], m[j][i]);
    return *this;
}

std::ostream& operator<<(std::ostream& os, const Mat3& v) {
    for (int i = 0; i < 3; ++i) {
        os << "| ";
        for (int j = 0; j < 3; ++j) {
            os << v.m[i][j] << " ";
        }
        os << "|\n";
    }
    return os;
}

float Mat3::Determinant() {
    return m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
         - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
         + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
}

//四维矩阵
Mat4::Mat4(){
    m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.f;
    m[0][1] = m[0][2] = m[0][3] = 
    m[1][0] = m[1][2] = m[1][3] = 
    m[2][0] = m[2][1] = m[2][3] =
    m[3][0] = m[3][1] = m[3][2] = 0.f;
}

Mat4::Mat4(float mat[4][4]){
    memcpy(m, mat, 16 * sizeof(float));
}

Mat4::Mat4(float t00, float t01, float t02, float t03,
         float t10, float t11, float t12, float t13,
         float t20, float t21, float t22, float t23,
         float t30, float t31, float t32, float t33){
    m[0][0] = t00, m[0][1] = t01, m[0][2] = t02, m[0][3] = t03;
    m[1][0] = t10, m[1][1] = t11, m[1][2] = t12, m[1][3] = t13;
    m[2][0] = t20, m[2][1] = t21, m[2][2] = t22, m[2][3] = t23;
    m[3][0] = t30, m[3][1] = t31, m[3][2] = t32, m[3][3] = t33;
}

Mat4 Mat4::operator*(const Mat4 &mat) const {
    Mat4 out;  
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            out.m[i][j] = 0.0f;
            for (int k = 0; k < 4; ++k) {
                out.m[i][j] += this->m[i][k] * mat.m[k][j];
            }
        }
    }
    return out;
}

Mat4 Mat4::operator*(float s) const {
    Mat4 out;  
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            out.m[i][j] = m[i][j] * s;
        }
    }
    return out;
}

std::ostream &operator<<(std::ostream &os, const Mat4 &v) {
    os << "|\t";
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            os << v.m[i][j];
            if (j < 3) os << "\t"; 
        }
        os << "\t|" << std::endl;
        if (i < 3) os << "|\t"; 
    }
    return os;
}

Mat4 &Mat4::Transpose() {
    for (int i = 0; i < 4; ++i) {
        for (int j = i + 1; j < 4; ++j) {
            std::swap(m[i][j], m[j][i]);
        }
    }
    return *this;
}

float Mat4::Determinant() const{
    float det = 0.0f;
    for (int col = 0; col < 4; ++col) {
        float sign = ((col % 2) == 0) ? 1.0f : -1.0f;
        det += sign * m[0][col] * Minor(0, col);
    }
    return det;
}

float Mat4::Minor(int row, int col) const {
    float sub[3][3];
    int subi = 0, subj = 0;

    for (int i = 0; i < 4; ++i) {
        if (i == row) continue;
        subj = 0;
        for (int j = 0; j < 4; ++j) {
            if (j == col) continue;
            sub[subi][subj] = m[i][j];
            subj++;
        }
        subi++;
    }

    Mat3 subMat(sub);
    return subMat.Determinant();
}

Mat4 Mat4::Inverse() const{
    Mat4 inv;
    float det = Determinant();
    if (std::fabs(det) < 1e-6f) {
        std::cerr << "Error: Matrix is singular and cannot be inverted." << std::endl;
        return inv; 
    }

    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            float sign = ((i + j) % 2 == 0) ? 1.0f : -1.0f;
            inv.m[j][i] = sign * Minor(i, j) / det;  
        }
    }

    return inv;
}

